/***
	*******************************************************************************************************************************************************************
	* @file    Encoder.c
	* @version V2.1
	* @date    2024-7-22
	* @author  御龙	
	* @brief   MSPM0G3507小车PID调试通用模板
   *************************************************************************************************
   *  @description
	*	
	*  只需要调用MEASURE_MOTORS_SPEED()函数，且Motor1_Speed和Motor2_Speed为电机实际速度
   *
>>>>> 其他说明：未经允许不可擅自转发、售卖本套代码，大家都是我国的有为青年，请保持好自己的初心，在此，向你表达我的感谢
	*************************************************************************************************************************************************************
***/
#include "Encoder.h"
#include "mpu6050.h"
#include "key.h"

int32_t Motor1_Encoder_Value=0;
int32_t Motor2_Encoder_Value=0;

float Motor1_Speed = 0;
float Motor2_Speed = 0;

uint32_t left_TIM_cur=0;
uint32_t left_TIM_pre=0;
uint32_t left_TIM_err=0;

uint32_t right_TIM_cur=0;
uint32_t right_TIM_pre=0;
uint32_t right_TIM_err=0;

volatile double Motor1_Lucheng,Motor2_Lucheng=0;
volatile double Measure_Distance = 0;


// //编码器1计算速度
// void Motor1_Get_Speed(void){
//     double Encoder_TIM = 0;
//     double Speed = 0;
//     Encoder_TIM= (double)DL_Timer_getTimerCount(QEI_LEFT_INST);
// 	if(Encoder_TIM>1000)
// 	{
// 		Encoder_TIM=Encoder_TIM-65535;
		
// 	}
//     DL_Timer_setTimerCount(QEI_LEFT_INST, 0);
//     Speed =Encoder_TIM/(CC)*PI*RR;//计算速度
//     Motor1_Speed = Speed/130.0;
// 	// if(fabsf(Motor1_Speed)>1000)
// 	// {
// 	// 	Motor1_Speed=0;
// 	// }
// }


//编码器2计算速度
// void Motor2_Get_Speed(void){
//     double Encoder_TIM = 0;
//     double Speed = 0;
//     Encoder_TIM=(double) DL_Timer_getTimerCount(QEI_RIGHT_INST);
// 	if(Encoder_TIM>1000)
// 	{
// 		Encoder_TIM=(Encoder_TIM-65535);
		
// 	}
//     DL_Timer_setTimerCount(QEI_RIGHT_INST, 0);
//     Speed =Encoder_TIM/(CC)*PI*RR;//计算速度
//     Motor2_Speed = Speed/130.0;
// 	// if(fabsf(Motor2_Speed)>1000)
// 	// {
// 	// 	Motor2_Speed=0;
// 	// }
// }





//测量所有电机速度
void MEASURE_MOTORS_SPEED(void){
	Motor1_Get_Speed();Motor2_Get_Speed();
	
	Motor1_Lucheng += Motor1_Speed*SAMPLE_TIME;//路程累计
	Motor2_Lucheng += Motor2_Speed*SAMPLE_TIME;//路程累计
	Measure_Distance = Motor1_Lucheng/2.0 +Motor2_Lucheng/2.0;
	
	// if(Measure_Distance>10000)
	// {
	// 	Measure_Distance = 0;
	// 	Motor1_Lucheng = 0;
	// 	Motor2_Lucheng = 0;
		
	// }
}



// 左马达编码器测速（优化溢出处理）
void Motor1_Get_Speed()
{
    // 1. 获取当前计数
    uint32_t current_count = DL_Timer_getTimerCount(QEI_LEFT_INST);
    
    // 2. 计算无符号差值（可能因溢出产生大数）
    uint32_t raw_diff = current_count - left_TIM_pre;
    
    // 3. 判断溢出类型并计算实际差值
    int32_t actual_diff;
    
    // 反转溢出检测：当前值远小于前值 → 计数值从0回环到0xFFFFFFFF
    if (current_count < left_TIM_pre && raw_diff > ENCODER_MAX / 2) {
        actual_diff = (int32_t)(current_count - left_TIM_pre);  // 实际为负差值
    }
    // 正转溢出检测：当前值远大于前值 → 计数值从0xFFFFFFFF回环到0
    else if (current_count > left_TIM_pre && raw_diff > ENCODER_MAX / 2) {
        actual_diff = (int32_t)(current_count - left_TIM_pre - ENCODER_MAX - 1);
    }
    // 正常情况（未溢出）
    else {
        actual_diff = (int32_t)raw_diff;
    }
    
    // 4. 计算速度（加入时间因子）
    double Speed = (double)actual_diff / (CC) * PI * RR;
    Motor1_Speed = Speed*0.0875;
    
    // 5. 更新前值
    left_TIM_pre = current_count;
}

// 右马达函数同理
void Motor2_Get_Speed()
{
    // 代码与左马达完全相同，只需替换变量名
    uint32_t current_count = DL_Timer_getTimerCount(QEI_RIGHT_INST);
    uint32_t raw_diff = current_count - right_TIM_pre;
    
    int32_t actual_diff;
    if (current_count < right_TIM_pre && raw_diff > ENCODER_MAX / 2) {
        actual_diff = (int32_t)(current_count - right_TIM_pre);
    }
    else if (current_count > right_TIM_pre && raw_diff > ENCODER_MAX / 2) {
        actual_diff = (int32_t)(current_count - right_TIM_pre - ENCODER_MAX - 1);
    }
    else {
        actual_diff = (int32_t)raw_diff;
    }
    
    double Speed = (double)actual_diff / (CC) * PI * RR;
    Motor2_Speed = Speed*0.0875;
    
    right_TIM_pre = current_count;
}